#include <core/test/planner_reactive_test.h>
#include <iostream>

using std::cout;
using std::cin;
using std::endl;

using PlayerCc::dtor;

static const double kPi = 3.14159265;

int main(int argc, char* argv[]) {
  player_point_2d initial_position, goal_position;
  double initial_yaw, goal_yaw;

  // Set up player's objects.
  PlayerCc::PlayerClient player_client("localhost");
  PlayerCc::Position2dProxy position2d_proxy(&player_client);
  PlayerCc::SonarProxy sonar_proxy(&player_client);
  //PathPlanner path_planner(argv[1], argv[2]);

  position2d_proxy.RequestGeom();
  sonar_proxy.RequestGeom();
  player_client.Read();

  // Get mission.
  /*
  cout << "Initial pose (px py pa). pa in degrees.: ";
  cin >> initial_position.px >> initial_position.py >> initial_yaw;
  initial_yaw = dtor(initial_yaw);
  cout << "Goal pose (px py pa): ";
  cin >> goal_position.px >> goal_position.py >> goal_yaw;
  goal_yaw = dtor(goal_yaw);
  */

  initial_position.px = 0;
  initial_position.py = 0;
  initial_yaw = dtor(0);

  //goal_position.px = 1.50;
  //goal_position.py = -4;
  goal_position.px = 2;
  goal_position.py = 0;
  // TODO goal_yaw = dtor(0);

  position2d_proxy.SetOdometry(initial_position.px, initial_position.py, initial_yaw);
  position2d_proxy.SetMotorEnable(1);
  
  // Create synchronized access current pose object.
  SyncedCurrentPose synced_current_pose;
  player_pose2d initial_pose = {initial_position.px, initial_position.py, initial_yaw};
  synced_current_pose.set_current_pose(initial_pose);

  // Create reactive behavior object.
  ReactiveBehavior reactive_behavior(&player_client, 
                                     &position2d_proxy, 
                                     &sonar_proxy, 
                                     &synced_current_pose);

  // Create localization estimation object and spawn associated thread.
  FakeLocalizationEstimator localization_estimator(&player_client, &position2d_proxy, &synced_current_pose);
  boost::thread localization_estimator_thread(
      boost::ref(localization_estimator));

  reactive_behavior.MoveToGoalAvoidingObstacles(goal_position);
  // TODO reactive_behavior.RotateTo(goal_yaw);

  localization_estimator_thread.join();
  // Note: program never ends.
  return 0;
}
